import serial
import struct
import time


class SensorFrame:
    FRAME_HEAD = 0xAA
    FRAME_TAIL = 0x55
    FRAME_TYPE_DATA = 0x03

    def __init__(self):
        self.device_id = 0
        self.angle_x = 0.0
        self.angle_y = 0.0
        self.angle_z = 0.0
        self.vib_level = 0

    @staticmethod
    def parse_frame(data):
        if len(data) != 13:  # 总长度检查
            return None

        if data[0] != SensorFrame.FRAME_HEAD or data[-1] != SensorFrame.FRAME_TAIL:
            return None

        # 校验和检查
        checksum = sum(data[1:-2]) & 0xFF
        if checksum != data[-2]:
            return None

        frame = SensorFrame()

        # 解析数据
        frame.device_id = (data[2] << 8) | data[3]

        # 解析角度值（转换回浮点数）
        angle_x = struct.unpack('>h', bytes([data[4], data[5]]))[0]
        angle_y = struct.unpack('>h', bytes([data[6], data[7]]))[0]
        angle_z = struct.unpack('>h', bytes([data[8], data[9]]))[0]

        frame.angle_x = angle_x / 100.0
        frame.angle_y = angle_y / 100.0
        frame.angle_z = angle_z / 100.0

        frame.vib_level = data[10]

        return frame


def main():
    # 配置串口
    ser = serial.Serial(
        port='COM4',  # 根据实际串口修改
        baudrate=115200,
        bytesize=serial.EIGHTBITS,
        parity=serial.PARITY_NONE,
        stopbits=serial.STOPBITS_ONE
    )

    buffer = bytearray()

    try:
        while True:
            if ser.in_waiting:
                # 读取串口数据
                data = ser.read()
                buffer.extend(data)

                # 查找完整帧
                while len(buffer) >= 13:
                    if buffer[0] == SensorFrame.FRAME_HEAD:
                        if len(buffer) >= 13 and buffer[12] == SensorFrame.FRAME_TAIL:
                            # 解析帧
                            print(buffer.hex())
                            frame = SensorFrame.parse_frame(buffer[:13])
                            if frame:
                                print(f"Device ID: 0x{frame.device_id:04X}")
                                print(f"Angles: X={frame.angle_x:.2f}, Y={frame.angle_y:.2f}, Z={frame.angle_z:.2f}")
                                print(f"Vibration Level: {frame.vib_level}")
                                print("-" * 40)
                            buffer = buffer[13:]
                        else:
                            buffer = buffer[1:]
                    else:
                        buffer = buffer[1:]

            time.sleep(0.01)  # 短暂休眠，避免CPU占用过高

    except KeyboardInterrupt:
        ser.close()
        print("\nProgram terminated by user")


if __name__ == "__main__":
    main()